#include"ARM_CM3.h"
#include"dap_access.h"
#include"shell.h"
#include"user_print.h"
typedef struct {
  volatile unsigned int FP_CTRL;
  volatile unsigned int FP_REMAP;
  // Number Implemented determined by FP_CTRL
  volatile unsigned int FP_COMP[];
} sFpbUnit;

static sFpbUnit *const FPB = (sFpbUnit *)0xE0002000;
unsigned int num_code_comparators;
unsigned int fp_ctrl;
void DebugInit(void)
{
    unsigned int stat;
    stat = AHB_Read(&(FPB->FP_CTRL),&fp_ctrl,4);
    if(stat == 0)
    {
        num_code_comparators = (((fp_ctrl >> 12) & 0x7) << 4) | ((fp_ctrl >> 4) & 0xF);
        user_printf("break num:%d\r\n",num_code_comparators);
    }
    else
    {
        user_printf("acquire FPB error\r\n");
    }
    stat = AHB_WriteWord(&(FPB->FP_CTRL),3);//enable fpb
    if(stat != 0 )
    {
        user_printf("fpb enable faild\r\n");
    }
}
unsigned int haltdebug(void)
{
    unsigned int stat,ret = 0,Data;
    stat = AHB_Read(DebugDHCSR,&Data,4);
    if(stat !=0 )
    {
        ret = 1;
        goto close;
    }
    Data = Data&0x2f;
    stat = AHB_WriteWord(DebugDHCSR,DebugDHCSR_WriteKey | Data | C_DEBUGEN | C_HALT);
    if(stat != 0)
    {
        ret = 2;
        goto close;
    }
    stat = AHB_WriteWord(DebugDHCSR,DebugDHCSR_WriteKey | Data | C_DEBUGEN | C_HALT | C_MASKINTS);
    if(stat != 0)
    {
        ret = 2;
        goto close;
    }
    close:
    return ret;
}
unsigned int QueryBreakPoint(unsigned int addr,unsigned int *QueryRes)
{
    unsigned int stat,ret=0,Data,i;
    *QueryRes = 0;//default current pc isn't bkpt
    for(i = 0; i < num_code_comparators; i++)
    {
        stat = AHB_Read(&(FPB->FP_COMP[i]),&Data,4);
        if(stat != 0)
        {
            ret = 1;
            break;
        }
        if((Data&0x3ffffffc) == (addr&(~3)))
        {
            if(Data&1)
            {
                ret = 0;
                *QueryRes = 1;
                break;
            }
        }
    }
    return ret;
}
SH_CMD(haltdebug,"enable halt debug and halt target");
unsigned int ResumeCPU()
{
    unsigned int stat,ret=0,Data,Pc;
    stat = CPU_RegRead(CM3_R15,&Pc);
    if(stat != 0)
    {
        ret = 1;
        goto close;
    }
    stat = AHB_Read(Pc,&Data,2);
    if(stat != 0)
    {
        ret = 1;
        goto close;
    }
    Data &= 0xff00;
    if(Data == 0xbe00)
    {
        Pc += 2;
        stat = CPU_RegWrite(CM3_R15,&Pc);
        if(stat != 0)
        {
            ret = 1;
            goto close;
        }
    }
    stat = QueryBreakPoint(Pc,&Data);
    if(stat != 0)
    {
        ret = 1;
        goto close;
    }
    if(Data)
    {
        if(0 != StepiCPU())
        {
            ret = 1;
            goto close;
        }
        if(0 != StepiCPU())
        {
            ret = 1;
            goto close;
        }
    }
    stat = AHB_Read(DebugDHCSR,&Data,4);
    if(stat !=0 )
    {
        ret = 1;
        goto close;
    }
    Data = Data&0x29;//clear Halt Step
    Data = Data|1;//enable halt debug
    stat = AHB_WriteWord(DebugDHCSR,DebugDHCSR_WriteKey | Data);
    if(stat != 0)
    {
        ret = 2;
        goto close;
    }
    close:
    return ret;
}
static void resume()
{
    if(ResumeCPU() == 0)
    {
        user_printf("cmd execute ok\r\n");
    }
    else
    {
        user_printf("cmd execute error\r\n");
    }
}
SH_CMD(resume,"resume cpu");
unsigned int StepiCPU(void)
{
    unsigned int stat,ret = 0,Data,retry = 0,Pc;
    stat = AHB_WriteWord(&(FPB->FP_CTRL),0x02);
    if(stat != 0)
    {
        ret = 11;
        goto close;
    }
    stat = CPU_RegRead(CM3_R15,&Pc);
    if(stat != 0)
    {
        ret = 1;
        goto close;
    }
    stat = AHB_Read(Pc,&Data,2);
    if(stat != 0)
    {
        ret = 1;
        goto close;
    }
    Data &= 0xff00;
    if(Data == 0xbe00)
    {
        Pc += 2;
        stat = CPU_RegWrite(CM3_R15,&Pc);
        if(stat != 0)
        {
            ret = 1;
            goto close;
        }
    }
    stat = AHB_Read(DebugDHCSR,&Data,4);
    if(stat !=0 )
    {
        ret = 1;
        goto close;
    }
    Data = Data&0x29;
    stat = AHB_WriteWord(DebugDHCSR,DebugDHCSR_WriteKey | Data | C_DEBUGEN | C_STEP);
    if(stat != 0)
    {
        ret = 2;
        goto close;
    }
    stat = LoopWordReadReady(DebugDHCSR);
    if(stat != 0)
    {
        ret = 3;
        goto close;
    }
    //Drop this vaule because this vaule is last value
    stat = AP_Read(DRW,&Data);
    if(SW_ACK_OK != stat)
    {
        user_printf("DRW DAP ret %d \r\n",stat);
        ret = 4u;
        goto close;
    }
    do
    {
        stat = AP_Read(DRW,&Data);
        if(SW_ACK_OK != stat)
        {
            user_printf("DRW DAP ret %d \r\n",stat);
            if(retry < 3)
            {
                ret = 5u;
                goto close;
            }
            retry++;
        }
        /* code */
    } while ((Data&S_HALT) == 0);
    stat = AHB_WriteWord(&(FPB->FP_CTRL),0x03);
    if(stat != 0)
    {
        ret = 55;
        goto close;
    }
    close:
    
    return ret;
}
static void step(void)
{
    if(StepiCPU() == 0)
    {
        user_printf("cmd execute ok\r\n");
    }
    else
    {
        user_printf("cmd execute error\r\n");
    }
}
SH_CMD(step,"stepi cpu");
unsigned int HaltCPU()
{
    unsigned int Data,stat,ret = 0,retry = 0;
    ADD_Mutx();
    stat = haltdebug();
    if(stat != 0)
    {
        ret = 1;
        goto close;
    }
    stat = LoopWordReadReady(DebugDHCSR);
    if(stat != 0)
    {
        ret = 2;
        goto close;
    }
    //Drop this vaule because this vaule is last value
    stat = AP_Read(DRW,&Data);
    if(SW_ACK_OK != stat)
    {
        user_printf("DRW DAP ret %d \r\n",stat);
        ret = 3u;
        goto close;
    }
    do
    {
        stat = AP_Read(DRW,&Data);
        if(SW_ACK_OK != stat)
        {
            user_printf("DRW DAP ret %d \r\n",stat);
            if(retry < 3)
            {
                ret = 4u;
                goto close;
            }
            retry++;
        }
        /* code */
    } while ((Data&S_HALT) == 0);
    
    close:
    Relase_Mutx();
    return ret;
}
unsigned int IsHalted(unsigned int *stat)
{
    unsigned int ret,Data;
    ret = AHB_Read(DebugDHCSR,&Data,4);
    if(ret == 0)
    {
        *stat = (Data&S_HALT) >> 17;
    }
    return ret;
}
unsigned int MemoryRead(unsigned int addr,unsigned char*buf,unsigned int len)
{
    return AHB_Read(addr,buf,len);
}
unsigned int MemoryWrite(unsigned int addr,unsigned char*buf,unsigned int len)
{
    return AHB_BlockWrite(addr,buf,len);
}
static cpu_s(void)
{
    unsigned int stat,data;
    stat = IsHalted(&data);
    if(stat == 0)
    {
        if(data)
        {
            user_printf("cpu is halted\r\n");
        }
        else
        {
            user_printf("cpu is running\r\n");
        }
    }
}
SH_CMD(cpu_s,"cpu state query");
// (type == 0)	/* memory breakpoint */
// (type == 1)	/* hardware breakpoint */
// (type == 2)	/* write watchpoint */
// (type == 3)	/* read watchpoint */
// (type == 4)	/* access watchpoint */
unsigned int InsertBreakpointOrWatchpoint(unsigned int type,unsigned int addr,unsigned int len)
{
    unsigned int i,ret = 1,stat,data;
    
    if(type == 0)
    {
        user_printf("attempt add soft bkpt\r\n");
    }
    else if(type == 1)
    {
        addr = addr&(~3);
        for(i = 0; i < num_code_comparators; i++)
        {
            stat = AHB_Read(&(FPB->FP_COMP[i]),&data,4);
            if(stat != 0)
            {
                user_printf("inset breakpoint error\r\n");
            }
            if((data&0x3fffffff) == (addr|1))
            {
                if(data&0xC0000000)
                {
                    ret = 0;
                    break;
                }
            }
            if( (data & 1) == 0)
            {
                stat = AHB_WriteWord(&(FPB->FP_COMP[i]),addr | 0xC0000001);
                if(stat == 0)
                {
                    ret = 0;
                }
                break;
            }
        }
    }
    return ret;
}
unsigned int RemovetBreakpointOrWatchpoint(unsigned int type,unsigned int addr,unsigned int len)
{
    unsigned int i,ret = 0,stat,data;
    if(type == 0)
    {
        user_printf("attempt remove soft bkpt\r\n");
    }
    else if(type == 1)
    {
        addr = addr&(~3);
        for(i = 0; i < num_code_comparators; i++)
        {
            stat = AHB_Read(&(FPB->FP_COMP[i]),&data,4);
            if(stat != 0)
            {
                user_printf("remove breakpoint error\r\n");
            }
            if((data&0x3fffffff) == (addr|1))
            {
                if(data&0xC0000000)
                {
                    stat = AHB_WriteWord(&(FPB->FP_COMP[i]),0);
                    if(stat != 0)
                    {
                        ret = 1;
                    }
                }
            }
        }
    }
    return ret;
}
unsigned int CPU_RegRead(unsigned int RegNumber,unsigned int *value)
{
    unsigned int stat,ret = 0,temp,retry = 0,Data;
    stat = IsHalted(&temp);
    if(stat != 0)
    {
        ret = 1;
        goto close;
    }
    if(temp)
    {
        //cpu is halt
        stat = AHB_WriteWord(DebugDCRSR,RegNumber&0x1fu);
        if(stat != 0)
        {
            ret = 2;
            goto close;
        }
        do
        {
            stat = AHB_Read(DebugDHCSR,&Data,4);
            if(stat != 0)
            {
                goto close;
            }
        } while ((Data&S_REGRDY) == 0);
        stat = AHB_Read(DebugDCRDR,&Data,4);
        if(stat != 0)
        {
            ret = 3;
            goto close;
        }
        *value = Data;
    }
    close:
    return ret;
}
unsigned int CPU_RegWrite(unsigned int RegNumber,unsigned int value)
{
    unsigned int stat,ret = 0,temp,retry = 0,Data;
    stat = IsHalted(&temp);
    if(stat != 0)
    {
        ret = 1;
        goto close;
    }
    if(temp)
    {
        //cpu is halt
        stat = AHB_WriteWord(DebugDCRDR,value);
        if(stat != 0)
        {
            ret = 3;
            goto close;
        }
        stat = AHB_WriteWord(DebugDCRSR,(RegNumber&0x1fu) | CM3_reg_w);
        if(stat != 0)
        {
            ret = 2;
            goto close;
        }
        
        do
        {
            stat = AHB_Read(DebugDHCSR,&Data,4);
            if(stat != 0)
            {
                goto close;
            }
        } while ((Data&S_REGRDY) == 0);
        
    }
    close:
    return ret;
}
unsigned int TargetRunCode(unsigned int reg[])
{
    unsigned int stat,ret = 0,i;
    for(i = 0; i < 17; i++)
    {
        stat = CPU_RegWrite(i,reg[i]);
        if(stat != 0)
        {
            ret = 1;
            goto close;
        }
    }
    stat = ResumeCPU();
    if(stat !=0 )
    {
        ret = 2;
        goto close;
    }
    close:
    return ret;
}
extern unsigned long long DebugTick;
unsigned int WaitRAMCodeFinish(unsigned int Sec)
{
    unsigned int timeout,stat,Data,ret = 0;
    timeout = DebugTick + 1000*Sec;
    stat = LoopWordReadReady(DebugDHCSR);
    if(stat != 0)
    {
        user_printf("Ram Run failed\r\n");
        ret = 1;
        goto close;
    }
    //Drop this vaule because this vaule is last value
    stat = AP_Read(DRW,&Data);
    if(SW_ACK_OK != stat)
    {
        user_printf("DRW DAP ret %d \r\n",stat);
        ret = 1;
        goto close;
    }
    do
    {
        stat = AP_Read(DRW,&Data);
        if(SW_ACK_OK != stat)
        {
            user_printf("DRW DAP ret %d \r\n",stat);
        }
        if(timeout < DebugTick)
        {
            ret = 2;
            break;
        }
    } while ((Data&S_HALT) == 0);
    close:
    return ret;
}
unsigned int GetRAMCodeReturn(unsigned int *res)
{
    unsigned int stat ,ret = 0;
    stat = CPU_RegRead(0,res);
    return stat;
}
static void reset(unsigned int argc,char*argv[])
{
    unsigned int stat,Data,reset_run = 0,retry = 0;
    if(argc == 2)
    {
        if(!strcmp(argv[1],"halt"))
        {
            stat = AHB_WriteWord( DebugDEMCR,0x7F1);
            if(stat != 0)
            {
                user_printf("DebugDEMCR write error\r\n");
            }
        }
        if(!strcmp(argv[1],"run"))
        {
            stat = AHB_WriteWord( DebugDEMCR,0);
            reset_run = 1;
            if(stat != 0)
            {
                user_printf("DebugDEMCR write error\r\n");
            }
        }
    }
    else
    {
        if(reset_run)
        {
            stat = AHB_WriteWord( DebugDHCSR,DebugDHCSR_WriteKey | 1);
            if(stat != 0)
            {
                user_printf("DebugDEMCR write error\r\n");
            }
        }
    }
    stat = AHB_WriteWord( DebugAIRCR,DebugAIRCR_Writekey | 3);
    if(stat != 0)
    {
        user_printf("DebugDEMCR write error\r\n");
    }
    else
    {
        stat = LoopWordReadReady(DebugDHCSR);
        if(stat == 0)
        {
            stat = AP_Read(DRW,&Data);
            do
            {
                stat = AP_Read(DRW,&Data);
                if(SW_ACK_OK != stat)
                {
                    user_printf("DRW DAP ret %d \r\n",stat);
                    if(retry < 3)
                    {
                        break;
                    }
                    retry++;
                }
            } while (0 == (Data&S_RESET_ST));
            if(retry < 3)
            {
                user_printf("execult success\r\n");
            }
        }
    }
}
SH_CMD(reset,"target reset operiation");
static void reg(unsigned int argc,char*argv[])
{
    unsigned int stat,reg_addr,data;
    if(argc == 2)
    {
        reg_addr = get_data(argv[1]);
        if((reg_addr != 20)&&(reg_addr > CM3_PSP))
        {
            reg_addr = CM3_R15;
        }
        stat = CPU_RegRead(reg_addr,&data);
        if(stat == 0)
        {
            user_printf("reg[%d] = 0x%08x\r\n",reg_addr,data);
        }
        else
        {
            user_printf("reg read error\r\n");
        }
    }
    else if(argc == 3)
    {
        reg_addr = get_data(argv[1]);
        data = get_data(argv[2]);
        if((reg_addr != 20)&&(reg_addr > CM3_PSP))
        {
            user_printf("reg addr error\r\n");
            return;
        }
        stat = CPU_RegWrite(reg_addr,data);
        if(stat == 0)
        {
            user_printf("reg[%d] = %d\r\n",reg_addr,data);
        }
    }
}
SH_CMD(reg,"cpu reg read or write");